/*
 * Copyright 2016 The Cartographer Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef CARTOGRAPHER_NODE_H
#define CARTOGRAPHER_NODE_H

#include <map>
#include <memory>
#include <set>
#include <unordered_map>
#include <unordered_set>
#include <vector>
#include <mutex>

#include "mapping/map_builder_interface.h"

#include "node/trajectory_options.h"
#include "node/node_options.h"
#include "node/node_constants.h"

#include "mapping/pose_extrapolator.h"

namespace cartographer {

struct LaserScan {  
    float angle_min; 
    float angle_max;        // end angle of the scan [rad]
    float angle_increment;  // angular distance between measurements [rad]
    double time_increment;   // time between measurements [seconds] 
    double scan_time;        // time between scans [seconds]
    float range_min;        // minimum range value [m]
    float range_max;        // maximum range value [m]
    std::vector<float> ranges;         // range data [m] (Note: values < range_min or > range_max should be discarded)
    std::vector<float> intensities;    // intensity data [device-specific units]. 

    std::vector<float> x_coords;
    std::vector<float> y_coords;
};

// Wires up to SLAM.
class Node {
 public:
  Node(const NodeOptions& node_options, const Eigen::Matrix4f& Tvl, std::shared_ptr<Publisher> publisher_ptr = nullptr);
  ~Node();

  // c++11: =delete: 禁止编译器自动生成默认函数; =default: 要求编译器生成一个默认函数

  // 禁止编译器自动生成 默认拷贝构造函数(复制构造函数)
  Node(const Node&) = delete;
  // 禁止编译器自动生成 默认赋值函数
  Node& operator=(const Node&) = delete;

  // Finishes all yet active trajectories.
  void FinishAllTrajectories();
  // Finishes a single given trajectory. Returns false if the trajectory did not
  // exist or was already finished.
  bool FinishTrajectory(int trajectory_id);

  // Runs final optimization. All trajectories have to be finished when calling.
  void RunFinalOptimization();

  // Starts the first trajectory with the default topics.
  void StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options);
  // Converts laser message to point cloud. Returns the time when the last point
  // was acquired (different from the ROS timestamp). Timing of points is given in
  // the fourth component of each point relative to `Time`.
  std::tuple<sensor::PointCloudWithIntensities, common::Time>
  ToPointCloudWithIntensities(const LaserScan& msg);
  // For sensor_msgs::LaserScan.
  bool HasEcho(float) { return true; }
  float GetFirstEcho(float range) { return range; }

  // The following functions handle adding sensor data to a trajectory.
  void HandleOdometryMessage(int trajectory_id, const std::string& sensor_id,
                             const sensor::OdometryData& odometry_data);
  void HandleImuMessage(int trajectory_id, const std::string& sensor_id,
                        const sensor::ImuData& imu_data);
  void HandleLaserScanMessage(const int& trajectory_id, const std::string& sensor_id,
                              const LaserScan& laser_scan);
  
  // Serializes the complete Node state.
  void SerializeState(const std::string& filename,
                      const bool include_unfinished_submaps);

 private:

  // Returns the set of SensorIds expected for a trajectory.
  // 'SensorId::id' is the expected ROS topic name.
  std::set<::cartographer::mapping::TrajectoryBuilderInterface::SensorId>
  ComputeExpectedSensorIds(const TrajectoryOptions& options) const;
  int AddTrajectory(const TrajectoryOptions& options);
  void LaunchSubscribers(const TrajectoryOptions& options, int trajectory_id);
  void AddExtrapolator(int trajectory_id, const TrajectoryOptions& options);
  void AddSensorSamplers(int trajectory_id, const TrajectoryOptions& options);

  const NodeOptions node_options_;

  std::mutex mutex_;

  std::unordered_set<std::string> subscribed_topics_;
  std::shared_ptr<mapping::MapBuilderInterface> map_builder_interface_ptr_; 

  // These are keyed with 'trajectory_id'.
  std::map<int, mapping::PoseExtrapolator> extrapolators_;

  std::shared_ptr<Publisher> publisher_ptr_; 

};

}  // namespace cartographer

#endif  // CARTOGRAPHER_NODE_H
